;--------------------------------------------------------
; nie testowany, generuje takiego samego hexa
; oprocz 00 34 00 34 na ko?cu.
;--------------------------------------------------------

	list p=16f873
	#INCLUDE <p16f873.inc>
;--------------------------------------------------------
; Equ
;--------------------------------------------------------

TEMP	EQU	20h

ZNAKB	EQU	22h



LLICZ	EQU	29h
HLICZ	EQU	2ah
STANP	EQU	2bh

DANA	EQU	2dh

STATBUF	EQU	2eh

TEMP1	EQU	2fh
TEMP2	EQU	30h

T1L     EQU 31h
T1H     EQU 32h

T2L     EQU 33h
T2H     EQU 34h

T3L     EQU 35h
T3H     EQU 36h

T4L     EQU 37h
T4H     EQU 38h

ERR     EQU 39h

MAL     EQU 40h
MAH     EQU 41h
MBL     EQU 42h
MBH     EQU 43h

;--------------------------------------------------------
; Code
;--------------------------------------------------------

    org    0x0

    GOTO start
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1

    org    0x4

    retfie

    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1



    org    0x10
Loopi:
    MOVLW 0x20
    MOVWF TEMP
    GOTO Loopp
SD:
SetupDelay:
Loop:

    CLRF TEMP
    CLRWDT
Loopp:
    DECFSZ TEMP, f
    GOTO Loopp
    RETURN

Loo1s:
    MOVLW 0x06
    MOVWF TEMP2
Loo1sa:
    MOVLW 0xD7
    MOVWF TEMP1
    CLRF TEMP

Loo1sp:
    CLRWDT
    CALL Loopp
    DECFSZ TEMP1, f
    GOTO Loo1sp
    DECFSZ TEMP2, f
    GOTO Loo1sa
    RETURN

Loo01s:
    MOVLW 0x81
    MOVWF TEMP1
    CLRF TEMP

Loo01sp:
    CLRWDT
    CALL Loop
    DECFSZ TEMP1, f
    GOTO Loo01sp
    RETURN


Loo008s:
    MOVLW 0x83
    MOVWF TEMP1 
    CLRF TEMP

Loo008sp:
    CLRWDT
    CALL Loop
    DECFSZ TEMP1, f
    GOTO Loo008sp
    RETURN

Loo02s:
    CLRF TEMP1
    CLRF TEMP
    GOTO Loo01sp

start:
    BCF STATUS, RP0  ;bcf status, rp0
    CLRF PORTB       ; clear portb
    CLRF PORTA       ; clear porta
    CLRF PORTC      ; clear portc
    BSF STATUS, RP0  ; bsf status, rp0
    MOVLW 0x07
    MOVWF ADCON1
    MOVLW 0xFF
    MOVWF TRISA
    MOVLW 0xD0
    MOVWF TRISB
    MOVLW 0xFF
    MOVWF TRISC

    MOVLW 0x00
    MOVWF SSPSTAT
    BCF STATUS, RP0
    MOVLW 0x36
    MOVWF SSPCON
    MOVLW 0x06
    MOVWF T1CON
    CLRF TEMP
    CLRF ZNAKB

    MOVF PORTA, w
    ANDLW 0x3F
    MOVWF STANP

    clrc
    RLF STANP, w
    BSF STATUS, RP0
    MOVWF SSPADD
    BCF STATUS, RP0
    BSF PORTB, 0
    BSF PORTB, 1

    CLRF STANP
    GOTO Czenai2

Pomcze:
    BCF PORTB, 1
    BTFSS ZNAKB, 7  ;0 - fast, 1 - slow
    GOTO szybki         ; goto szybki
    CALL Loo1s
    CALL Loo1s
    CALL Loo02s
    CALL Loo01s
    GOTO pomiar_nie_pilot
szybki:
    CALL Loo02s
    CLRF TMR1L
    CLRF TMR1H
    BSF T1CON, TMR1ON
    CALL Loo02s
    BCF T1CON, TMR1ON
    MOVF TMR1L, w
    MOVWF LLICZ
    MOVF TMR1H, w
    MOVWF HLICZ
    MOVF HLICZ, w
    SUBLW 0x00          ; 0x00 - HLICZ
    BTFSS STATUS, Z  ; STATUS, Z ?
    GOTO pomiar_nie_pilot
    MOVF LLICZ, w
    SUBLW 0xE0
    BTFSS STATUS, C
    GOTO pomiar_nie_pilot
    MOVF LLICZ, w
    SUBLW 0xB0
    BTFSC STATUS, C
    GOTO pomiar_nie_pilot
    BSF STANP, 5    ;
    BSF PORTB, 2
    GOTO pomiar

pomiar_nie_pilot:
    BCF STANP, 5
    BCF PORTB, 2
pomiar:
    CLRF ERR

    CALL Loo01s
    CLRF TMR1L
    CLRF TMR1H
    BSF T1CON, TMR1ON

    ;////// dummy code
    MOVF TMR1L, w
    MOVWF T1L
    MOVF TMR1H, w
    MOVWF T1H
    ;////// dummy code

   
    CALL Loo02s
    CALL Loo01s

    MOVF TMR1L, w
    MOVWF T1L
    MOVF TMR1H, w
    MOVWF T1H

    CALL Loo02s
    CALL Loo01s

    MOVF TMR1L, w
    MOVWF T2L
    MOVF TMR1H, w
    MOVWF T2H

    CALL Loo02s
    CALL Loo01s

    MOVF TMR1L, w
    MOVWF T3L
    MOVF TMR1H, w
    MOVWF T3H

    BTFSC PORTB, 6
    BSF ERR , 0

    CALL Loo008s

    BCF T1CON, TMR1ON
    MOVF TMR1L, w
    MOVWF LLICZ
    MOVF TMR1H, w
    MOVWF HLICZ
    BCF STATUS, C
    RLF LLICZ, f
    RLF HLICZ, f

    ;MOVLW 0x00
    ;MOVWF ERR
    BTFSS ERR,0
    goto pom2
    CLRF LLICZ
    CLRF HLICZ
pom2:
    BTFSS PORTB, 6
    GOTO pomiar_ok
    CLRF LLICZ
    CLRF HLICZ
pomiar_ok:
    call pomiar_check
    BCF PORTB, 5
    BCF ZNAKB, 2
    BSF PORTB, 1
    BTFSC ZNAKB, 7  ; 0 fast, 1 slow
    GOTO Czenai2

    BCF STANP, 0
    MOVF HLICZ, w
    SUBLW 0x0B
    BTFSC STATUS, Z
    BSF STANP, 0
    GOTO Czenai2

Program:

Czenai2
    CLRWDT
    BTFSC ZNAKB, 2
    GOTO Pomcze
Niepom
    BTFSC PIR1, SSPIF
    CALL transi2c             ; call transi2c
    GOTO Czenai2

transi2c:
    BCF PIR1, SSPIF
    BSF STATUS, RP0
    MOVF SSPSTAT, w
    BCF STATUS, RP0
    ANDLW 0x2D
    MOVWF STATBUF
    MOVLW 0x09
    XORWF STATBUF, w
    BTFSS STATUS, Z
    GOTO state2
    MOVF SSPBUF, w

    BCF SSPCON, WCOL
    BCF SSPCON, SSPOV

    BSF SSPCON, CKP
    RETURN


state2:
    MOVLW 0x29
    XORWF STATBUF, w
    BTFSS STATUS, Z
    GOTO State3 ; STATE3 ?
    MOVF SSPBUF, w
    MOVWF DANA
    BCF SSPCON, WCOL
    BCF SSPCON, SSPOV
    BCF PORTB, 3
    BTFSC DANA, 3 ;line on/off
    BSF PORTB, 3
    BCF ZNAKB, 7   ; fast/slow
    BTFSC DANA, 1   ;sensor nr
    BSF ZNAKB, 7
    BTFSC DANA, 2   ;meas yes/no
    CALL Pomtak   ; pomtak ?
    BTFSC DANA, 0
    CALL TAKTAK  ; taktak ?
    BSF SSPCON, CKP
    RETURN


State3:
    MOVLW 0x0C
    XORWF STATBUF, w
    BTFSS STATUS, Z
    GOTO state4
    BCF SSPCON, WCOL
    GOTO Write


state4:
    MOVLW 0x2C
    XORWF STATBUF, w
    BTFSS STATUS, Z
    GOTO State5
    BCF SSPCON, WCOL
    BCF SSPCON, SSPOV
    GOTO Write2

State5:
    MOVLW 0x28
    XORWF STATBUF, w
    BTFSS STATUS, Z
    BCF PORTB, 0
    BSF SSPCON, CKP
    RETURN

Write:
    BSF STATUS, RP0
Wricz:
    BTFSC SSPSTAT, BF
    GOTO Wricz

    BCF STATUS, RP0
Wrecz:
    BCF SSPCON, WCOL
    MOVF LLICZ, w
    MOVWF SSPBUF
    BTFSC SSPCON, WCOL
    GOTO Wrecz
    BCF ZNAKB, 1        ; 2 or 3 b send i2c
    BSF SSPCON, CKP
    RETURN


Write2:
    BTFSC ZNAKB, 1
    GOTO Write3
    BSF STATUS, RP0
Wricz2:
    BTFSC SSPSTAT, BF
    GOTO Wricz2

    BCF STATUS, RP0
Wrecz2:
    BCF SSPCON, WCOL
    MOVF HLICZ, w
    MOVWF SSPBUF
    BTFSC SSPCON, WCOL
    GOTO Wrecz2
    BSF ZNAKB, 1    ; 2 or 3b
    BSF SSPCON, CKP
    RETURN

Write3:
    BSF STATUS, RP0
Wricz3:
    BTFSC SSPSTAT, BF
    GOTO Wricz3

    BCF STATUS, RP0
Wrecz3:
    BCF SSPCON, WCOL
    BCF STANP, 1
    BTFSS PORTB, 4
    BSF STANP, 1
    MOVF STANP, w
    MOVWF SSPBUF
    BTFSC SSPCON, WCOL
    GOTO Wrecz3
    BSF SSPCON, CKP
    RETURN

Pomtak:   ; pomtak
    BSF ZNAKB, 2    ; meas yes,no
    BTFSC ZNAKB, 7  ; fast, slow
    GOTO Skip_fast
    CALL Loo1s
    MOVLW 0x03
    MOVWF TEMP2
    CALL Loo1sa
    MOVLW 0x03
    MOVWF TEMP2
    CALL Loo1sa
    CLRF LLICZ
    CLRF HLICZ
    BSF PORTB, 5
    RETURN

Skip_fast:
    BSF PORTB, 5
    MOVLW 0x03
    MOVWF TEMP2
    CALL Loo1sa
    CLRF LLICZ
    CLRF HLICZ
    RETURN

TAKTAK:          ; taktak ?
    BSF SSPCON, CKP
    BSF PORTB, 5
    MOVLW 0x01
    MOVWF TEMP2
    CALL Loo1sa
    BCF PORTB, 5
    MOVLW 0x01
    MOVWF TEMP2
    CALL Loo1sa
    BSF PORTB, 5
    MOVLW 0x01
    MOVWF TEMP2
    CALL Loo1sa
    BCF PORTB, 5
    RETURN

pomiar_check:

    MOVF T2L,w
    MOVWF MAL
    MOVF T2H,w
    MOVWF MAH

    MOVF T3L,w
    MOVWF MBL
    MOVF T3H,w
    MOVWF MBH

    call minus
    MOVF MBL,w
    MOVWF T3L
    MOVF MBH,w
    MOVWF T3H
    ;/////////////// T3 = t3 - t2

    MOVF T1L,w
    MOVWF MAL
    MOVF T1H,w
    MOVWF MAH

    MOVF T2L,w
    MOVWF MBL
    MOVF T2H,w
    MOVWF MBH
    ; sub T2 - T1
    call minus
    MOVF MBL,w
    MOVWF T2L
    MOVF MBH,w
    MOVWF T2H

;///////////////// t2, t1 should be about equall now
test:
    MOVLW 0x07
    MOVWF MAL
    MOVLW 0x00
    MOVWF MAH

    MOVF T3L,w
    MOVWF MBL
    MOVF T3H,w
    MOVWF MBH
   
    call plus

    MOVF T2L,w
    MOVWF MAL
    MOVF T2H,w
    MOVWF MAH

    call minus

    MOVF MBH,1
    BTFSS STATUS,Z      ; MBH not zero after substraction
    goto pomiarErr

    BTFSS MBL,7
    goto next1
    movlw 0xFF
    xorwf MBL,1
    incf MBL,1      ; U2-> | MODUL| Neg, inc1

next1:
    movf MBL,0
    sublw 0x19
    BTFSS STATUS,C  ; MAH > 32 - err
    goto pomiarErr

test2:
    MOVLW 0x07
    MOVWF MAL
    MOVLW 0x00
    MOVWF MAH

    MOVF T2L,w
    MOVWF MBL
    MOVF T2H,w
    MOVWF MBH

    call plus

    MOVF T1L,w
    MOVWF MAL
    MOVF T1H,w
    MOVWF MAH

    call minus

    MOVF MBH,1
    BTFSS STATUS,Z      ; MBH not zero after substraction
    goto pomiarErr

    BTFSS MBL,7
    goto next2
    movlw 0xFF
    xorwf MBL,1
    incf MBL,1      ; U2-> | MODUL| Neg, inc1

next2:
    movf MBL,0
    sublw 0x19
    BTFSS STATUS,C  ; MAH > 32 - err
    goto pomiarErr

    ;all ok - proceed
    return
    
pomiarErr:

    BSF ERR, 1
    CLRF LLICZ
    CLRF HLICZ
    return

minus:

    ;b = b - a
    movf MAL,w
    subwf MBL, f
    movf MAH,w
    btfss STATUS,C
        incfsz MAH,w
    subwf MBH,f
    return

plus:

   ;b = b + a
    movf MAL,w
    addwf MBL,f
    btfsc STATUS, C
    incf MBH,f
    movf MAH,w
    addwf MBH,f
    return


    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1
    FILL 0x3FFF, 1

    END
